#include "PID.h"
#include "stm32f10x.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

// copied from TIMRegister.c
static inline uint16_t _TIM_GetAutoreload(TIM_TypeDef *timAddr) {
  assert_param(IS_TIM_ALL_PERIPH(timAddr));
  return (timAddr->ARR + 1);
}

PID *new_PID(void) {
  PID *this = (PID *)malloc(sizeof(PID));
  PID_Init(this);
  return this;
};
void delete_PID(PID *this) { free(this); };

void PID_Init(PID *this) {
  memset(this, 0, sizeof(PID));
  this->kp = 1;
  this->ti = (float)19260817;
  this->td = 0;
  this->cell = 100;
  this->floor = 0;
  this->intCell = (float)19260817;
  this->intFloor = -(float)19260817;
  this->actionMode = PID_POSITIVE_EFFECT;
  this->holdOn = 0;
};
void PID_Config(PID *this, float kp, float ti_ms, float td_ms, int T_ms,
                PIDActionMode mode, TIM_TypeDef *timer) {
  this->kp = kp;
  this->ti = ti_ms;
  this->td = td_ms;
  this->period = T_ms;
  this->actionMode = mode;
  this->_timer = timer;

  uint32_t _TIMIntPeriod_us =
      TIM_DEFAULT_PERIOD_us * _TIM_GetAutoreload(timer); // 计时器置数中断周期
  this->_TIMReloadCount = T_ms * 1000 / _TIMIntPeriod_us;
}
void PID_LimitEnable(PID *this) { this->limitEnabled = 1; };
void PID_LimitDisable(PID *this) { this->limitEnabled = 0; };
// config PID will auto enable limiting
void PID_LimitConfig(PID *this, float cell, float floor) {
  this->cell = cell;
  this->floor = floor;
  PID_LimitEnable(this);
}

void PID_IntLimitEnable(PID *this) { this->intLimitEnabled = 1; }
void PID_IntLimitDisable(PID *this) { this->intLimitEnabled = 0; }
// config PID will auto enable limiting
void PID_IntLimitConfig(PID *this, float cell, float floor) {
  this->intCell = cell;
  this->intFloor = floor;
  PID_IntLimitEnable(this);
}

void PID_SetMeasured(PID *this, float value) { this->measured = value; }
void PID_SetTarget(PID *this, float value) { this->target = value; }

/**
 * @brief Manual set the output of the PID.
 *
 * @param this Pointer to the PID
 * @param value The output value
 *
 * This function will set the output of the PID and turn on the hold mode.
 * The hold mode means that the PID will not calculate the output again until
 * the hold mode is turned off.
 */
void PID_SetOutput(PID *this, float value) {
  this->output = (this->actionMode == PID_NEGATIVE_EFFECT ? value : -value);
  this->holdOn = 1;
}

float PID_GetMeasured(PID *this) { return this->measured; }
float PID_GetTarget(PID *this) { return this->target; }
float PID_GetOutput(PID *this) {
  return this->actionMode == PID_NEGATIVE_EFFECT ? this->output : -this->output;
}

void PID_HoldOn(PID *this) { this->holdOn = 1; }
void PID_HoldOff(PID *this) {
  this->_err = this->_prevErr = 0;
  this->_int = 0;
  this->holdOn = 0;
}
void PID_Clear(PID *this) {
  this->_err = this->_prevErr = 0;
  this->_int = 0;
  // this->output = 0; // avoid sudden jump
  this->_currentCount = 0;
};

/**
 * @brief Step function for PID.
 *
 * PID will run at given period.
 * This function is supposed to be called in a interrupt routine.
 * Directly execute PID_Step() in its count interrupt to refresh the PID, no
 * need for additional frequency division.
 */
void PID_Step(PID *this) {
  this->_currentCount++;
  if (this->_currentCount >= this->_TIMReloadCount) {
    this->_currentCount = 0;

    if (!(this->holdOn)) {
      this->_prevErr = this->_err;
      this->_err = this->target - this->measured;

      if (this->ti == 0 || this->ti >= (float)19260817)
        this->_int = 0;
      else {
        this->_int += this->_err * (this->kp * (this->period / this->ti));
      }

      // limit integral
      if (this->intLimitEnabled) {
        this->_int = fmax(fmin(this->_int, this->intCell), this->intFloor);
      }

      // calculate output
      this->output = this->kp * this->_err // 比例项
                     + this->_int          // 积分项
                     + (this->kp * (this->td / this->period)) *
                           (this->_err - this->_prevErr); // 微分项

      // limit output
      if (this->limitEnabled) {
        this->output = fmax(fmin(this->output, this->cell), this->floor);
      }
      this->output = this->actionMode == PID_NEGATIVE_EFFECT ? this->output
                                                             : -this->output;
    }
  }
}